//
//  atan2.c
//  
//
//  Created by Bram Drenthe on 13-12-13.
//
//

#include <stdio.h>

#include "qmath.h"
#include <math.h>

static const q32_t FOUR_DIV_PI  = 0x145F3;            /*!< Fix16 value of 4/PI */
static const q32_t _FOUR_DIV_PI2 = 0xFFFF9840;        /*!< Fix16 value of -4/PI² */
static const q32_t X4_CORRECTION_COMPONENT = 0x399A;  /*!< Fix16 value of 0.225 */
static const q32_t PI_DIV_4 = 0x0000C90F;             /*!< Fix16 value of PI/4 */

static const q32_t THREE_PI_DIV_4 = 0x00025B2F;       /*!< Fix16 value of 3PI/4 */

q32_t qMul(q32_t a, q32_t b)
{
    long long product = (long long)a * b;
    
    if (product < 0) product--;
    q32_t result = product >> 16;
    result += (product & 0x8000) >> 15;
    return result;
}

q32_t qDiv(q32_t a, q32_t b)
{
    return (q32_t)( (((long long)a)<<(16))/b );
}

void generateArcTable()
{
    int i;
    
    int prev_arctan = 0;
    int prev_arctani = 0;
    
    int nom = 65536;
    int prev_d = nom;
    int INC = 16;
    int maxError = 256;
    
    float arctan;
    q32_t qarctan;
    
    for (i=0;i<65536*1256;i+=INC)
    {
        arctan = atanf((float)i/65536.0f);
        
        if (arctan >= 1.5707f) break;
        qarctan = qFromFloat(arctan);
        
        q32_t qarctancalc = prev_arctan + ((long long)i-(long long)prev_arctani) * (long long)nom / prev_d;
        
        int error = qarctancalc - qarctan;
        if (error < -maxError) printf("XXX %i ",-error); //error = -error;
        if (error > maxError)
        {
            arctan = atanf((float)i/65536.0f);
            qarctan = qFromFloat(arctan);
            
            int diffX = i - prev_arctani;
            int diffY = qarctan - prev_arctan;
            
            diffX *= nom;
            
            prev_d = diffX / diffY;
            
            printf("else if (v < %i) v = (%i+(v-%i)*%i/%i); /* %i */\n",i,prev_arctan,prev_arctani,nom,prev_d, maxError);
            maxError = maxError * 6 / 5;
            
            nom = nom / 2;
            if (nom < 1) nom = 1;
            prev_arctan = qarctan;
            prev_arctani = i;
            prev_d = ((long long)i*(long long)i/65536*nom/65536) + nom;
            
            //INC = INC + INC / 2;
        }
        
//        printf("%i;%.16f;%.16f;%i;%i\n",i,arctan,floatFromQ(qarctancalc),qarctancalc-qarctan, prev_d);
    }
    
    arctan = atanf((float)i/65536.0f);
    qarctan = qFromFloat(arctan);
    
    int diffX = i - prev_arctani;
    int diffY = qarctan - prev_arctan;
    
    diffX *= nom;
    
    prev_d = diffX / diffY;
    
    printf("else if (v < %i) v = (%i+(v-%i)*%i/%i); /* %i */\n",i,prev_arctan,prev_arctani,nom,prev_d, maxError);
    
    printf("else v = 102943;\n");
    printf("return vo > 0 ? v : -v;\n");
    printf("%i\n\n",i/65536);
}

int main()
{
    generateArcTable();
    return 0;
}